避障规划介绍及使用实例

1 简介

在很多应用中如汽车喷涂，果园采摘等都需要对运动轨迹进行避障规划以满足实际工况需求以及降低示教的复杂度

正常轨迹点A到B，中间有障碍物，不进行避障规划会以轨迹1进行运动最终导致机械臂与障碍物碰撞，而开启避障规划则会以轨迹2进行运动绕开障碍物最终到达目标点B

算法库目前支持多个轨迹点的避障规划，接口如下：

/**
 * @brief 规划生成无碰撞路径，目前仅支持关节空间
 * @param in_points: 描述路径几何信息的 N 个路径点(N >= 2)
 * @param obstacle_avoidance_params: 避障属性，包括避障算法类型、规划步长以及是否使能路径简化功能
 * @param tool_workpiece: 工具和工件信息
 * @param out_points: 规划输出的无碰撞路径点
 * @return: 返回值 < 0 表示添加失败, 具体定义查询return_value_definition.hpp
 */
ARAL_API_COMMON(1.0) int tpGenerateCollisionFreePoints(const std::vector<PathPoint>& in_points, const ObstacleAvoidanceParameters& obstacle_avoidance_params, const ToolWorkpiece& tool_workpiece, std::vector<PathPoint>& out_points) = 0;

• 避障算法：默认采用 RRTCONNECT 算法，具有高效及高准确率的特点，能够应用于大多数复杂工况场景下，算法库还支持RRT，LBKPIECE等算法
• 规划步长：即从起始点到目标点中间状态树中生成树一次的最大步长
• 路径简化：即路点之间是否需要进行简化，可在避障前提下减少多余路径点并平滑路径

struct ObstacleAvoidanceParameters
{
    ObstacleAvoidanceAlgorithm   avoid_type{ObstacleAvoidanceAlgorithm::RRTCONNECT};  // 避障算法（默认RRTCONNECT算法）
    double                       maxDistance;                                         // 规划步长
    bool                         simplify{true};                                      // 使能路径简化功能
}; // 避障路径规划属性

输入始末点（需要保证无碰撞），通过上面接口输出无碰撞路径点，然后通过tpAddpoints接口以B样条形式去执行

    /**
     * @brief 向规划器中添加多点运动路径, 具体的类型参考PathProperty.CurveProperty.type(关节空间或笛卡尔空间)
     * @param points: 描述路径几何信息的路径点(id指的第一段路径的id, 后面的id依次递增)
     * @param path_property: 路径对应的几何属性
     * @param move_property: 路径对应的运动属性
     * @param tool_workpiece: 工具和工件信息
     * @return: 返回值 < 0 表示添加失败, 具体定义查询 return_value_definition.hpp
     */
    ARAL_API_COMMON(1.0) int tpAddPoints(const std::vector<PathPoint>& points, const PathProperty& path_property, const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece) = 0;

2 示例

//! 1.设置B样条的运动属性
std::vector<interface::RLJntArray> joints;
std::vector<double> maxV = {3.11049,3.11049,3.11049,3.11018,3.11018,3.11018};
std::vector<double> maxA = {31.1049,31.1049,31.1049,20,20,20};
std::vector<double> maxJ = {248.839,248.839,248.839,160,160,160};
interface::PathProperty path_property;
path_property.describe_space = interface::DescribeSpace::JOINT;
path_property.cur_prop.type = interface::CurveType::BSPLINE;
interface::MoveProperty move_property;
move_property.maxV = maxV;
move_property.maxA = maxA;
move_property.maxJ = maxJ;

//! 2.给定避障规划的输入，如果起始点已设置，则只需要设置途径点（如果有）以及终点
joints = {{4.04642, 0.715765, 2.45813, 0.19917, 1.57034, -1.8009}, // 起始点
              {3.78491, -0.509789, 0.987052, -0.328536, 1.68902, -2.57394}}; // 目标点
int N  = joints.size();
std::vector<interface::PathPoint> points, out_points;
for(size_t i = 0; i < N - 1; i++)
    points.push_back(generatePathPoint(interface::DescribeSpace::JOINT, i+1, joints[i + 1], {}));

Setup(getRobotType(type.second));
initiatePlanner(robot, joints[0], t_w);

//! 3.设置机械臂连杆对应的碰撞几何体模型
interface::RLPose origin = {-0.4, -0.2, 0.3, 0, 0, 0};
geometric_shapes::Box* box = new geometric_shapes::Box(0.25, 0.25, 0.25);
geometric_shapes::ShapePtr shape = static_cast<geometric_shapes::ShapePtr>(box);
std::string link_name = "world";
int ret = robot->mdlSetLinkCollisionGeometryModel(link_name, {shape}, {origin});
CHECK(ret >= 0);

//! 4.规划生成无碰撞路径点
interface::ObstacleAvoidanceParameters obstacle_avoidance_params;
ret = robot->tpGenerateCollisionFreePoints(points, obstacle_avoidance_params, t_w, out_points);
CHECK(ret >= 0);

//! 5.采用B样条进行轨迹规划
ret = robot->tpAddPoints(out_points, path_property, move_property, t_w);
CHECK(ret >= 0);

std::vector<interface::TrajectoryPoint> plan_points;
std::vector<double> t;
JointState joint_state;
ret = generateTrajectoryPoints(robot, joints[0], 0.005, t_w, plan_points, joint_state, t);
CHECK(ret >= 0);